#include <motor.h>

#define DEBUG

Motor motor_Driver(2,3,4,5);
int echoPin = 11;
int triggerPin = 12;
int right_Count = 0;
void setup() {
    #ifdef DEBUG
        Serial.begin(115200);
    #endif  
    pinMode(triggerPin, OUTPUT);
    pinMode(echoPin, INPUT);
    delay(10); 
}

void loop() {
    right_Count = 0;
    long temp = getDistance();
    #ifdef DEBUG
        Serial.print("Ping: ");
        Serial.println(temp );
    #endif

/*
I started this section with if else loops. The turns were very wide.
So i changed it to while but i have not tested it yet.
I changed the delay at the end from 5 to 500 ms so that may help also.
*/

    while(temp > 20 || temp == 0){//I want to make a sharper right
        right_Count++;
        #ifdef DEBUG
            Serial.print("Right Count: ");
            Serial.println(right_Count);
        #endif
        if(right_Count < 5){
            #ifdef DEBUG
                Serial.println("Turning right!");
            #endif
            motor_Driver.turn_right();
            delay(50);
            motor_Driver.drive_forward();
            delay(50);
            motor_Driver.stop();
            delay(1);
            temp = getDistance();
            #ifdef DEBUG
                Serial.print("Ping: ");
                Serial.println(temp );
            #endif
        }else{
            temp = 18;            
            #ifdef DEBUG
                Serial.print("EXITING THE LOOP!!!!");
            #endif
        }        
    }
    while(temp < 15){//I want to make a sharper left
        #ifdef DEBUG
            Serial.println("Turning left!");
        #endif
        motor_Driver.turn_left();
        delay(50);
        motor_Driver.drive_forward();
        delay(50);
        motor_Driver.stop();
        delay(1);
        temp = getDistance();
        #ifdef DEBUG
            Serial.print("Ping: ");
            Serial.println(temp );
        #endif 
    }

    #ifdef DEBUG
        Serial.println("Driving forward!");/*base case is driving forward*/
    #endif
    motor_Driver.drive_forward();
        delay(500);
}

/*THIS HAS TO MOVE TO A LIBRARY!!!*/
/*
Gets the distance from an object and converts it to cm.
There are other functions and librarys that do thus.
They take up close to 10% of the total memory.
*/
long getDistance(){
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 100000);

  // convert the time into a distance
  return (duration / 58.2)  /*29 / 2*/;
  
}
